#include "Int_MPU6050.h"

// 六轴数据偏移量
AccelGyro_Struct accel_gyro_bias = {0};

void Int_MPU6050_WriteByte(uint8_t reg, uint8_t data)
{
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR_W, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, 1000);
}

void Int_MPU6050_WriteBytes(uint8_t reg, uint8_t *data, uint8_t len)
{
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR_W, reg, I2C_MEMADD_SIZE_8BIT, data, len, 1000);
}

uint8_t Int_MPU6050_ReadByte(uint8_t reg)
{
    uint8_t data;
    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR_R, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, 1000);
    return data;
}

void Int_MPU6050_ReadBytes(uint8_t reg, uint8_t *data, uint8_t len)
{
    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR_R, reg, I2C_MEMADD_SIZE_8BIT, data, len, 1000);
}

void Int_MPU6050_CalculateBias(void)
{
    // 1.确保处在静止状态:连续100次，前后两个角速度插值应该在100以内
    uint8_t count            = 0;
    Gyro_Struct last_gyro    = {0};
    Gyro_Struct current_gyro = {0};
    while (count < 100) {
        Int_MPU6050_ReadGyro(&current_gyro);
        if ((my_abs(current_gyro.x - last_gyro.x) < 100) && (my_abs(current_gyro.y - last_gyro.y) < 100) && (my_abs(current_gyro.z - last_gyro.z) < 100)) {
            count++;
        } else {
            count = 0;
        }
        last_gyro = current_gyro;
        HAL_Delay(2);
    }

    // 2.计算偏移量
    int32_t bias_sum[6]         = {0};
    AccelGyro_Struct accel_gyro = {0};
    for (uint16_t i = 0; i < 300; i++) {
        Int_MPU6050_ReadAccelGyro(&accel_gyro);
        bias_sum[0] += (accel_gyro.accel.x - 0);
        bias_sum[1] += (accel_gyro.accel.y - 0);
        bias_sum[2] += (accel_gyro.accel.z - 16384);

        bias_sum[3] += (accel_gyro.gyro.x - 0);
        bias_sum[4] += (accel_gyro.gyro.y - 0);
        bias_sum[5] += (accel_gyro.gyro.z - 0);
        HAL_Delay(2);
    }

    accel_gyro_bias.accel.x = bias_sum[0] / 300;
    accel_gyro_bias.accel.y = bias_sum[1] / 300;
    accel_gyro_bias.accel.z = bias_sum[2] / 300;

    accel_gyro_bias.gyro.x = bias_sum[3] / 300;
    accel_gyro_bias.gyro.y = bias_sum[4] / 300;
    accel_gyro_bias.gyro.z = bias_sum[5] / 300;
}

void Int_MPU6050_Init(void)
{
    // 1.复位->延时->唤醒
    Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x80);
    HAL_Delay(100);
    Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x00);

    // 2.设置陀螺仪和加速度计的量程
    Int_MPU6050_WriteByte(MPU_GYRO_CFG_REG, 3 << 3);
    Int_MPU6050_WriteByte(MPU_ACCEL_CFG_REG, 0x00);

    // 3.设置低通滤波器（DLPF）
    Int_MPU6050_WriteByte(MPU_CFG_REG, 0x01);

    // 4.设置采样率分频寄存器
    Int_MPU6050_WriteByte(MPU_SAMPLE_RATE_REG, 0x01);

    // 5.关闭中断
    Int_MPU6050_WriteByte(MPU_INT_EN_REG, 0x00);

    // 6.关闭 AUX IIC 接口、禁止 FIFO
    Int_MPU6050_WriteByte(MPU_USER_CTRL_REG, 0x00);

    // 7.配置时钟源
    Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x01);

    // 8.使能加速度计和陀螺仪
    Int_MPU6050_WriteByte(MPU_PWR_MGMT2_REG, 0x00);

    // 9.测试
    // uint8_t id = Int_MPU6050_ReadByte(MPU_DEVICE_ID_REG);
    // printf("id:%x\n", id);

    // 9.计算六轴数据的偏移量
    Int_MPU6050_CalculateBias();
}

void Int_MPU6050_ReadAccel(Accel_Struct *accel)
{
    uint8_t buffer[6];
    Int_MPU6050_ReadBytes(MPU_ACCEL_XOUTH_REG, buffer, 6);
    accel->x = buffer[0] << 8 | buffer[1];
    accel->y = buffer[2] << 8 | buffer[3];
    accel->z = buffer[4] << 8 | buffer[5];
}

void Int_MPU6050_ReadGyro(Gyro_Struct *gyro)
{
    uint8_t buffer[6];
    Int_MPU6050_ReadBytes(MPU_GYRO_XOUTH_REG, buffer, 6);
    gyro->x = buffer[0] << 8 | buffer[1];
    gyro->y = buffer[2] << 8 | buffer[3];
    gyro->z = buffer[4] << 8 | buffer[5];
}

void Int_MPU6050_ReadAccelGyro(AccelGyro_Struct *accel_gyro)
{
    // 1.获取六轴数据
    Int_MPU6050_ReadAccel(&accel_gyro->accel);
    Int_MPU6050_ReadGyro(&accel_gyro->gyro);

    // 2.处理偏移问题
    accel_gyro->accel.x -= accel_gyro_bias.accel.x;
    accel_gyro->accel.y -= accel_gyro_bias.accel.y;
    accel_gyro->accel.z -= accel_gyro_bias.accel.z;
    accel_gyro->gyro.x -= accel_gyro_bias.gyro.x;
    accel_gyro->gyro.y -= accel_gyro_bias.gyro.y;
    accel_gyro->gyro.z -= accel_gyro_bias.gyro.z;
}